package roborally.model;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

import roborally.exceptions.IllegalMoveException;
import roborally.exceptions.InvalidPositionException;
import roborally.exceptions.PositionSharingException;
import roborally.pathfinding.PathFinding;

import be.kuleuven.cs.som.annotate.*;

/**
 * A class for dealing with robots.
 * 
 * @invar	The orientation of each robot must be a legal
 *          orientation for a robot.
 *          | canHaveAsOrientation(getOrientation())
 * @invar	The amount of energy of each robot must be a legal
 *         	amount of energy for a robot.
 *          | canHaveAsEnergy(getEnergy())
 * 
 * @version 1.0
 * @author  Pieter Robberechts	(1e Bach Informatica)
 * @author	Jeroen Sanders		(1e Bach Informatica)
 */
public class Robot extends Element implements IEnergyHolder {

	/**
	 * Initialize this new robot with given orientation and energy.
	 * 
	 * @param 	orientation
	 * 			The orientation for this new robot.
	 * @param 	initialEnergy 
	 * 			The initial amount of energy for this new robot.
	 * @pre		The given amount of energy must be a valid amount of energy for a robot.
	 * 			| canHaveAsEnergy(initialEnergy)
	 * @effect	The given orientation is set as the orientation of this robot.
	 * 			| setOrientation(orientation)
	 * @effect	The given energy is set as the energy of this robot.
	 * 			| setEnergy(initialEnergy)
	 */
	public Robot(Orientation orientation, EnergyAmount initialEnergy){
		assert(canHaveAsEnergy(initialEnergy));
		setOrientation(orientation);
		setEnergy(initialEnergy);
	}

	/**
	 * Set the orientation for this robot to the given orientation.
	 * 
	 * @param 	orientation
	 * 			The orientation for the robot. 
	 * @post   	If the given orientation is a valid orientation, 
	 * 			the orientation of this robot is set to the given orientation.
	 *       	| if ( canHaveAsOrientation(orientation) )
	 *       	|   then new.getOrientation() == orientation
	 * @post   	If the given orientation is an invalid orientation,
	 *         	the orientation of this robot remains unchanged.
	 *       	| if ( ! canHaveAsOrientation(orientation) )
	 *       	|   then new.getOrientation() == getOrientation()     
	 */
	@Model @Raw
	private void setOrientation(Orientation orientation) {
		if (canHaveAsOrientation(orientation))
			this.orientation = orientation;
	}
	
	/**
	 * Variable registering the orientation of this robot.
	 */
	private Orientation orientation = Orientation.RIGHT;
	
	/**
	 * Set the amount of energy of this robot to the given amount of energy.
	 * 
	 * @param 	energyAmount
	 * 			The amount of energy for the robot.
	 * @pre		The given amount of energy must be a valid amount of energy.
	 * 			| canHaveAsEnergy(energyAmount)
     * @post   	The amount of energy of this robot is equal to the given amount of energy.
     *       	| new.getEnergy().isIdenticalTo(energyAmount)
	 */
	@Model @Raw
	private void setEnergy(EnergyAmount energyAmount) {
		assert canHaveAsEnergy(energyAmount); 
		this.energyAmount = energyAmount;		
	}
	
	/**
	 * Variable registering the amount of energy of this robot.
	 */
	private EnergyAmount energyAmount;

	/**
	 * Return the maximal amount of energy for this robot.
	 */
	@Basic @Immutable
	public EnergyAmount getMaxEnergy(){
		return maxEnergy;
	}
	
	/**
	 * Variable registering the maximal amount of energy of this robot.
	 */
	private final EnergyAmount maxEnergy = new EnergyAmount(20000);
	
	/**
     * Check whether this robot can have the given amount of energy as its maximum amount of energy.
     * 
     * @param  	maxEnergy
     * 		   	The amount of energy to check.
     * @return 	True if and only if the given amount of energy is positive (exclusive zero)
     * 			and less than or equal to 20000 Ws.
     *       	| result == ( maxEnergy != null && maxEnergy.moreThan(new EnergyAmount(0)) && !maxEnergy.moreThan(new EnergyAmount(20000)) )
	 */
	@Raw
	public boolean canHaveAsMaxEnergy(EnergyAmount maxEnergy){
		return ( maxEnergy != null && maxEnergy.moreThan(new EnergyAmount(0)) && !maxEnergy.moreThan(new EnergyAmount(20000)) );
	}
	
	/**
	 * Return the amount of energy of this robot.
	 */
	@Basic @Raw @Override
	public EnergyAmount getEnergy(){
		return energyAmount;
	}
	
	/**
     * Check whether the given amount of energy is a valid amount of energy for this robot.
     * 
     * @param  	energy
     * 		   	The amount of energy to check.
     * @return 	True if and only if the given amount of energy is effective
     * 			and less than or equal to the maximal amount of energy for a robot.
     *       	| result == ( energy != null && !energy.moreThan(getMaxEnergy()) )
	 */
	@Raw
	public boolean canHaveAsEnergy(EnergyAmount energy){
		return ( energy != null && !energy.moreThan(getMaxEnergy()) );
	}
	
	/**
	 * Recharges the robot with a given amount of energy.
	 * 
	 * @param 	energyAmount
	 * 		  	| The amount of energy to add.
	 * @pre		The given amount of energy to be recharged must be effective.
	 * 			| energyAmount != null
	 * @pre 	The given amount of energy to be recharged in combination with the actual amount of energy of the robot 
	 * 			must be a valid amount energy for a robot.
	 * 			| canHaveAsEnergy( getEnergy().add(energyAmount) )
	 * @effect	The robots energy will be increased by the given amount of energy.
	 * 			| setEnergy( getEnergy().add(energyAmount) )
	 */
	@Override
	public void recharge(EnergyAmount energyAmount){
		assert( energyAmount != null );
		assert( canHaveAsEnergy( getEnergy().add(energyAmount) ) );
		setEnergy( getEnergy().add(energyAmount) );
	}
	
	/**
	 * Return the orientation of this robot. 
	 */
	@Basic @Raw 
	public Orientation getOrientation(){
		return orientation;
	}
	
	/**
	 * Check whether the given orientation is a valid orientation for this robot.
	 * 
	 * @param  	orientation
	 *         	The orientation to check.
	 * @return 	True if the orientation is UP, RIGHT, DOWN or LEFT
	 * 			| result == (orientation == Orientation.UP || orientation == Orientation.RIGHT ||
	 *			|		  	orientation == Orientation.DOWN || orientation == Orientation.LEFT)
	 */
	@Raw
	public boolean canHaveAsOrientation(Orientation orientation){
		return (orientation == Orientation.UP || orientation == Orientation.RIGHT ||
				orientation == Orientation.DOWN || orientation == Orientation.LEFT);
	}
	
	/**
	 * Move a robot on position forward.
	 * 
	 * @pre		The robot must have enough energy to move.
	 * 			| ! getEnergy().lessThan(getEnergyRequiredToMove())
	 * @post	The robot moves one position forward in its current orientation.
	 * 			| new.getPosition() == getPosition().getNeighbour(getOrientation())
	 * @post	The robot loses the required amount of energy to move.
	 * 			| new.getEnergy() == (getEnergy().subtract(getEnergyRequiredToMove()))
	 * @throws	IllegalMoveException
	 * 			The robot moves to a coordinate which is an invalid coordinate.
	 * 			| getBoard() != null && !getBoard().isValidPosition(getPosition().getNeighbour(getOrientation()))
	 * @throws	IllegalMoveException
	 * 			The robot's board or position is NULL.
	 * 			| getBoard() == null || getPosition() == null
	 * @throws	IllegalMoveException
	 * 			The robot can't share a position with one or more elements on the destination.
	 * 			| ! board.canHaveElementOnPosition(this, position.getNeighbour(getOrientation()))
	 * @throws	IllegalStateException
	 * 			The robot is terminated.
	 * 			| isTerminated()
	 * 
	 */
	public void move() throws IllegalStateException, IllegalMoveException {
		assert( ! getEnergy().lessThan(getEnergyRequiredToMove()) );
		
		if(isTerminated())
			throw new IllegalStateException("The robot is terminated!");
		
		try {
			Board board = getBoard();
			Position position = getPosition();
			if(board.canHaveElementOnPosition(this, position.getNeighbour(getOrientation()))){
				board.putElement(position.getNeighbour(getOrientation()), this);
				setEnergy(getEnergy().subtract(getEnergyRequiredToMove()));
			} else
				throw new IllegalMoveException("This robot and one or more elements on the destination can't share the same position.");
		} catch (NullPointerException e) {
			throw new IllegalMoveException("This robot isn't associated with a board.");
		} catch (InvalidPositionException e) {
			throw new IllegalMoveException("The destination is an invalid position.");
		} catch (IllegalArgumentException e) {
			e.printStackTrace();
		} catch (PositionSharingException e) {
			e.printStackTrace();
		}
	}
	
	/**
	 * Returns the amount of energy required for a robot to make a single move.
	 * 
	 * @return 	The amount of energy required to move.
	 * 			| result == MOVE_ENERGY.add(new EnergyAmount(WEIGHT_MOVE_ENERGY.getAmountInUnits()*getLoad().getWeightInUnits()/1000)
	 */
	@Model
	public EnergyAmount getEnergyRequiredToMove() {
		return MOVE_ENERGY.add( new EnergyAmount(WEIGHT_MOVE_ENERGY.getAmountInUnits()*getLoad().getWeightInUnits()/1000) );
	}
	
	/**
	 * Constant registering the basic amount of energy required for a robot to move.
	 */
	public static final EnergyAmount MOVE_ENERGY = new EnergyAmount(500);
	
	/**
	 * Constant registering the amount of energy needed to move 1 kilogram.
	 */
	public static final EnergyAmount WEIGHT_MOVE_ENERGY = new EnergyAmount(50);
	
	/**
	 * Turns the robot in a given direction.
	 * 
	 * @param	Direction
	 * 			The direction of the robot's turn.
	 * @pre		The robot must have enough energy to turn.
	 * 			| ! getEnergy().lessThan(getEnergyRequiredToTurn())
	 * @pre		The given direction should be effective.
	 * 			| direction != null
	 * @effect	The robot's orientation has changed following the given direction.
	 * 			| setOrientation(getOrientation().turn90(direction))
	 * @effect	The robot loses energy for turning.
	 * 			| setEnergy(getEnergy().subtract(getEnergyRequiredToTurn()))
	 * 
	 */
	public void turn(Direction direction){
		assert( ! getEnergy().lessThan(getEnergyRequiredToTurn()) );
		assert(direction != null);
		setOrientation(getOrientation().turn90(direction));
		setEnergy(getEnergy().subtract(getEnergyRequiredToTurn()));
	}
	
	/**
	 * Returns the amount of energy required for a robot to make a single turn.
	 * 
	 * @return	The amount of energy required to turn.
	 * 			| result == TURN_ENERGY
	 */
	@Model @Immutable
	public static EnergyAmount getEnergyRequiredToTurn(){
		return TURN_ENERGY;
	}
	
	/**
	 * Constant registering the amount of energy required for a robot to make a single
	 * turn of 90 degrees.
	 */
	public static final EnergyAmount TURN_ENERGY = new EnergyAmount(100);
	
	/**
	 * Return the total weight of all the items the robot is carrying.
	 * 
	 * @return	The sum of all the items their weight.
	 * 			| sum{ for each item in getItems() : if( ! item.isTerminated() ) then item.getWeight() }
	 */
	@Model
	private Weight getLoad(){
		Weight weight = new Weight();
		for(Item item : getItems())
				weight.add(item.getWeight());
		return weight;
	}
	
	/**
	 * Returns a Set containing all the items the robot is carrying.
	 * When the robot carries no items the returned set will be empty.
	 */
	@Basic
	public Set<Item> getItems(){
		return new HashSet<Item>(items);
	}
	
	/**
	 * Variable referencing a list collecting all the items,
	 * obtained by this robot.
	 * 
	 * @invar  	The referenced list is effective.
	 *       	| items != null
	 * @invar  	Each item registered in the referenced list is effective and not terminated.
	 *       	| for each item in items: (item != null) && ( ! item.isTerminated() )
	 * @invar  	No item is registered at several positions
	 *         	in the referenced list.
	 *       	| for each I,J in 0..items.size()-1:
	 *       	|   ( (I == J) || ( items.get(I) != items.get(J) )
	 * @invar	The list is sorted by ascending weight of the items in it.
	 * 			| for each I,J in 0..items.size()-1:
	 *       	|   if (I < J) then ( items.get(I).getWeight() <= items.get(J).getWeight() )      
	 */
	private final List<Item> items = new ArrayList<Item>();
	
	/**
	 * Return the ith heaviest item a robot is carrying in constant time.
	 * 	The first item is indicated with 1.
	 * 
	 * @param	i
	 * 			Represents the ith heaviest item.
	 * @return	Return the ith heaviest item this robot is carrying.
	 * 			| result == items.get(i)
	 * @throws	IllegalArgumentException
	 * 			'i' should be a value between 1 and items.size().
	 * 			| i < 1 && i > items.size()
	 */
	public Item getIthHeaviestItem(int i) throws IllegalArgumentException {
		if(i < 1)
			throw new IllegalArgumentException("The minimal value for 'i' is one.");
		if(i > items.size())
			throw new IllegalArgumentException("The maximal value for 'i' is" + items.size() + ".");
		return items.get(i-1);
	}
	
	/**
	 * Return a textual representation of this robot.
	 * 
	 */
	@Override
	public String toString(){
		if(isTerminated())
			return "This robot is terminated.";
		if(getPosition() != null)
			return "Robot [Energy: " + getEnergy() + " , Position: " + getPosition().toString() + "]";
		else
			return "Robot [Energy: " + getEnergy() + " ]";
	}
	
	/**
	 * Let the robot pick up a battery, if the battery is located on the same position and board.
	 * 
	 * @param	item
	 * 			The item the robot has to pick up.
	 * @effect	If the item is on the exact same position and board as the robot,  
	 * 			the item is removed from the board and added to the robots inventory.
	 * 			| if(item != null && item.getBoard() != null && item.getPosition().equals(getPosition()) && item.getBoard().equals(getBoard()))
	 * 			|		item.getBoard().removeElement(item)
	 * 			|		items.add(item)
	 * 			|		Collections.sort(items)
	 * 
	 * @post	If the item was successfully picked up, the robot carries the item.
	 * 			| if(item != null && item.getBoard() != null && item.getPosition().equals(getPosition()) && item.getBoard().equals(getBoard()))
	 * 			|	then getItems().contains(item)
	 */
	public void pickUp(Item item) {
		if(item != null && item.getBoard() != null && item.getPosition().equals(this.getPosition()) && item.getBoard().equals(this.getBoard())) {
			item.getBoard().removeElement(item);
			items.add(item); 
			Collections.sort(items);
			item.setInUse(true);
		}
	}
	
	/**
	 * Let the robot use an item.
	 * 
	 * @param	item
	 * 			The item the robot has to use.
	 * @effect	If the robot carries the given item, the state of the robot is changed 
	 * 			depending on the specific effect of the given item.
	 * 			| if( getItems().contains(item) && ! item.isTerminated() )
	 * 			| 	then item.use(this)
	 * @effect	If the item is terminated after using it, the robot should drop it.
	 * 			| if( getItems().contains(item) && item.isTerminated() )
	 * 			| 	then drop(item)
	 */
	public void use(Item item) {
		if(!isTerminated() && getItems().contains(item)) {
			item.use(this);
		}
	}
	
	/**
	 * Let the robot drop an item.
	 * 
	 * @param	item
	 * 			The item the robot has to drop.
	 * @post	The item is removed from the inventory of this robot.
	 * 			| ! getItems().contains(item)
	 * @effect	If the robot is carrying the item and if the robot is placed on a board, 
	 * 			the item must be placed at the position where the robot is located; 
	 * 			otherwise if the robot isn't placed on a board, the item simply has no owner and is not located on any board.
	 * 			| if(getItems().contains(item) && getBoard() != null && !item.isTerminated() && getBoard().canHaveElementOnPosition(item, getPosition()))
	 * 			|		getBoard().putElement(getPosition(),item)
	 * 
	 */
	public void drop(Item item){
		if(getItems().contains(item)) {
			items.remove(item);
			item.setInUse(false);
			try {
				getBoard().putElement(getPosition(), item);
			} catch (NullPointerException e) {
				// The robot isn't placed on a board ->
				// the item simply has no owner and is not located on any board
			} catch (IllegalStateException e) {
				e.printStackTrace();
			} catch (IllegalArgumentException e) {
				e.printStackTrace();
			} catch (InvalidPositionException e) {
				e.printStackTrace();
			} catch (PositionSharingException e) {
				// The item can't share a position with one of the elements on the current
				// position of the robot -> it shouldn't be placed on a board
			}
		}
	}
	
	/**
	 * Return the amount of energy required to shoot.
	 */
	@Basic @Immutable
	public EnergyAmount getEnergyRequiredToShoot(){
		return new EnergyAmount(1000);
	}
	
	/**
	 * Let a robot shoot its laser in the direction it is facing.
	 * 	Starting from the robot and moving in the direction of its current orientation, 
	 * 	the first position containing one or more robots, walls or batteries is hit.
	 * 	If the robot is not placed on a board, shooting has no effect.
	 * 
	 * @pre 	The robot should have enough energy to shoot.
	 * 			| ! getEnergy().lessThan(getEnergyRequiredToShoot())
	 * @effect	Starting from the robot and moving in the direction of its current orientation,
	 * 			the first encountered element is terminated.
	 * 			| getBoard().getFirstElementStartingAt(getPosition(), getOrientation()).terminate();
	 * @effect	The amount of energy for this robot is set to its current amount minus the amount required to shoot.
	 * 			| setEnergy(getEnergy().subtract(getEnergyRequiredToShoot()))
	 */
	public void shoot(){
		assert( ! getEnergy().lessThan(getEnergyRequiredToShoot()) );

		try {
			getBoard().getFirstElementStartingAt(getPosition(), getOrientation()).terminate();
			setEnergy(getEnergy().subtract(getEnergyRequiredToShoot()));
		} catch(NullPointerException e) {
			// The robot is not placed on a board or no element is hit
		} catch (IllegalArgumentException e) {
			e.printStackTrace();
		} catch (InvalidPositionException e) {
			e.printStackTrace();
		}
	}
	
	@Override
	public boolean canSharePositionWithCore(Element other){
		return (other instanceof Item);
	}
	
	/**
	 * Transfer energy from a robot to another energy holder.
	 * 
	 * @param	other
	 * 			The energy holder to transfer an amount of energy to.
	 * @param	amount
	 * 			The amount of energy to transfer.
	 * 
	 * @pre		The other energyholder shouldn't be NULL.
	 * 			| other != null
	 * @pre		The amount of energy shouldn't be NULL.
	 * 			| amount != null
	 * @pre		The other energyholder shouldn't be terminated.
	 * 			| ! other.isTerminated()
	 * @pre		The other energyholder must be able to hold his current
	 * 			amount of energy plus the transfered amount.
	 * 			| other.canHaveAsEnergy( getEnergy().add(amount) )
	 * @pre		The robot should have enough energy to transfer this amount of energy.
	 * 			| this.canHaveAsEnergy( getEnergy().subtract(amount) )
	 * 
	 * @effect	The robot's amount of energy will be set to his current
	 * 			amount minus the transfered amount of energy.
	 * 			| setEnergy(getEnergy().subtract(amount))
	 * @effect	The other energyholder's amount of energy will be recharged with
	 * 			transfered amount of energy.
	 * 			| other.recharge(amount)
	 */
	@Override
	public void transferEnergy(IEnergyHolder other, EnergyAmount amount){
		assert (other != null);
		assert !other.isTerminated();
		assert (amount != null);
		assert other.canHaveAsEnergy( other.getEnergy().add(amount) );
		assert this.canHaveAsEnergy( getEnergy().subtract(amount) );
		setEnergy(getEnergy().subtract(amount));
		other.recharge(amount);
	}
	
	/**
	 * Return the minimal amount of energy to reach a particular position.
	 * 
	 * @param	position
	 * 			The destination for the robot.
	 * 
	 * @return	The minimal energy required for this robot the reach the destination.
	 * 			| result == path.getEnergyRequired()
	 * 
	 * @throws	InvalidPositionException
	 * 			The given position is invalid.
	 * 			| !getBoard().isValidPosition(position)
	 * @throws	IllegalArgumentException 
	 * 			The given position is NULL.
	 * 			| position == null
	 */
	public double getMinimalCostToReach(Position position) throws InvalidPositionException, IllegalArgumentException{
		// Checks whether the robot can stand on the destination.
		if(!this.getBoard().canHaveElementOnPosition(this, position))
			return -1;
		// Construct a path for this robot towards the destination.
		PathFinding path = new PathFinding(this,position);
		// No path has been found.
		if(!path.pathFound())
			return -1;
		// The robot has not enough energy to reach the destination.
		if(path.getEnergyRequired().moreThan(getEnergy()))
			return -2;
		// Return the minimal energy required.
		return path.getEnergyRequired().getAmountInUnits();
	}
	
	/**
	 * Moves two robots located on the same board as close as possible towards each other.
	 * 
	 * @param	other
	 * 			The other robot which is involved in the movement.
	 * 
 	 * @post	Both robots end up as close as possible to each other (in optimal case next to each other), 
	 * 			taking into account their initial amounts of energy and with a minimal amount of consumed energy.
	 * 			| for each Position1, Position2:
	 * 			|	if	( getBoard().canHaveElementOnPosition(Position1,this) && getBoard().canHaveElementOnPosition(Position2,other) &&
	 * 			|		  this.getEnergy() >= this.getMinimalCostToReach(Position1) && other.getEnergy() >= other.getMinimalCostToReach(Position2) 
	 * 			|	   	)
	 * 			|		then( 	(Position1.getManhattanDistance(Position2) > new.this.getPosition().getManhattanDistance(new.other.getPosition()) ||
	 * 			|			 	(Position1.getManhattanDistance(Position2) == new.this.getPosition().getManhattanDistance(new.other.getPosition()) &&
	 * 			|				this.getMinimalCostToReach(Position1) + other.getMinimalCostToReach(Position2)	>= 
	 * 			|				this.getMinimalCostToReach(new.this.getPosition()) + other.getMinimalCostToReach(new.other.getPosition()) ) 
	 * 			|			)
	 * 
	 * @throws	IllegalArgumentException
	 * 			The other robot is NULL.
	 * 			| other == null
	 * @throws	IllegalArgumentException
	 * 			The other robot is this robot.
	 * 			| other == this
	 * @throws	IllegalStateException
	 * 			The two robots aren't located on the same board.
	 * 			| this.getBoard() != other.getBoard()
	 */
	public void moveNextTo(Robot other) throws IllegalArgumentException, IllegalStateException {
		
		// Check if the other robot is not null.
		if(other == null)
			throw new IllegalArgumentException("The other robot is NULL!");
		
		// Check if the robot does not try to move next to itself.
		if(other == this)
			throw new IllegalArgumentException("The other robot is this robot!");
		
		// Check if one of the robots is terminated
		if(this.isTerminated())
			throw new IllegalStateException("This robot is terminated!");
		if(other.isTerminated())
			throw new IllegalStateException("The other robot is terminated!");
		
		// Check whether the two robots are located on the same board.
		if(this.getBoard() != other.getBoard())
			throw new IllegalStateException("The other robot is on a different board!");
		
		// Check which robot has the most energy.
		if(other.getEnergy().moreThan(this.getEnergy())) {
			other.moveNextTo(this);
			return;
		}
		
		// Calculate a path from this robot to the other.
		PathFinding thisPath = new PathFinding(this,other);
		PathFinding otherPath;
		
		// Check whether a path has been found.
		if(thisPath.pathFound()) {
			otherPath = new PathFinding(other,this);
			
			// The path which requires the least energy will be used as the main path.
			// The other path will be the reversed version of the main path.
			if(thisPath.getEnergyRequired().lessThan(otherPath.getEnergyRequired())){
				otherPath = thisPath.reverse(other);
			} else {
				thisPath = otherPath.reverse(this);
			}
			
			// Calculate the 2 positions with the minimal energy required.
			Position thisEndPosition = null;
			Position loopPosition = thisPath.getClosestOnPath(getEnergy());
			Double minEnergy = null;
			// Loop through all the possible pairs of positions on the path.
			// And search for the pair with the least needed energy.
			while (loopPosition != null) {
				if(!otherPath.getEnergyRequired(otherPath.getPrevious(loopPosition)).moreThan(other.getEnergy())) {
					Double currentEnergy = thisPath.getEnergyRequired(loopPosition).getAmountInUnits() + 
							otherPath.getEnergyRequired(otherPath.getPrevious(loopPosition)).getAmountInUnits();
					if(minEnergy == null || (currentEnergy < minEnergy) ) {
						minEnergy = currentEnergy;
						thisEndPosition = loopPosition;
					}
				} else {
					break;
				}
				loopPosition = thisPath.getPrevious(loopPosition);
			}
			
			// Check if a pair of positions has been found.
			if(thisEndPosition != null) {
				// The robots can get next to each other.
				try {
					// Change this robot's position, energy and orientation.
					if( !thisEndPosition.equals(this.getPosition())) {
						this.getBoard().putElement(thisEndPosition, this);
						this.setEnergy(this.getEnergy().subtract(thisPath.getEnergyRequired(this.getPosition())));
						this.setOrientation(thisPath.getOrientation(this.getPosition()));
					}
					// Change the other robot's position, energy and orientation.
					if( !otherPath.getPrevious(thisEndPosition).equals(other.getPosition())) {
						this.getBoard().putElement(otherPath.getPrevious(thisEndPosition), other);
						other.setEnergy(other.getEnergy().subtract(otherPath.getEnergyRequired(other.getPosition())));
						other.setOrientation(otherPath.getOrientation(other.getPosition()));
					}
				} catch (IllegalStateException e) {} catch (InvalidPositionException e) {} catch (PositionSharingException e) {}
				return;
			}
			
		}
		
		// If no path has been found or there wasn't enough energy for the robots
		// to move next to each other, get the robots as close as possible.
		try {
			// Move this robot as close as possible.
			this.getBoard().putElement(thisPath.getClosest(this.getEnergy()), this);
			this.setEnergy(this.getEnergy().subtract(thisPath.getEnergyRequired(this.getPosition())));
			this.setOrientation(thisPath.getOrientation(this.getPosition()));
			// Calculate a path for the other robot to this robot's new position.
			otherPath = new PathFinding(other, this);
			// Move the other robot as close as possible.
			this.getBoard().putElement(otherPath.getClosest(other.getEnergy()), other);
			other.setEnergy(other.getEnergy().subtract(otherPath.getEnergyRequired(other.getPosition())));
			other.setOrientation(otherPath.getOrientation(other.getPosition()));
		} catch (IllegalStateException e) {} catch (InvalidPositionException e) {} catch (PositionSharingException e) {}
		
	}

}
